

#include "fh_mc_dma.h"
#include "mol_hw_rsa.h"
#include <asm/arch/hardware.h>

#define swab32(x) \
    ((unsigned long)( \
        (((unsigned long)(x) & (unsigned long)0x000000ffUL) << 24) | \
        (((unsigned long)(x) & (unsigned long)0x0000ff00UL) <<  8) | \
        (((unsigned long)(x) & (unsigned long)0x00ff0000UL) >>  8) | \
        (((unsigned long)(x) & (unsigned long)0xff000000UL) >> 24) ))

//#define swab32(x) (x)




#define RSA_CFG 			0x0
#define DMA_REQ_CFG1		0x4
#define RSA_DEBUG			0x8
#define RSA_INT			    0xc
#define RSA_D				0x210
#define RSA_D_X(x)			(RSA_D + ((x) << 3))
#define RSA_N				0x10
#define RSA_N_X(x)			(RSA_N + ((x) << 3))
#define RSA_M				0x410
#define RSA_M_X(x)			(RSA_M + ((x) << 3))
#define RSA_RESULT0			(0x610)
#define RSA_RESULT(x)		(RSA_RESULT0 + ((x) << 2))
#define RSA_BIGDIV_CFG		0x810
#define RSA_ESCAN_CFG0		0x814
#define RSA_ESCAN_CFG1		0x818

#define RSA_MODE			BIT(0)
#define RSA_DMA_EN			BIT(1)
#define RSA_TYPE			BIT(2)
#define RSA_START			BIT(3)
#define RSA_BUS_SEL_OFS		4
#define RSA_M_SIGN			BIT(7)
#define RSA_4096			(1 << 0)
#define RSA_2048			(0 << 0)
#define RSA_ENCRY			(1 << 2)
#define RSA_DECRY			(0 << 2)
#define RSA_M_NEGTIVE		(1 << 7)
#define RSA_M_ACTIVE		(0 << 7)


#define DMA_RX_REQ_SET_SW	BIT(0)
#define DMA_RX_REQ_CLR_SW	BIT(1)
#define INT_SET_SW			BIT(2)
#define INT_CLR_SW			BIT(3)

#define DIVISOR_SHIFT_CNT_CFG_OFS	0
#define DIVISOR_SHIFT_CNT_CFG_MASK	0x0000003f

#define EBIT_LEN_CNT_CFG_OFS		0
#define EBIT_LEN_CNT_CFG_MASK		0x0000003f
#define BIT_LEN_OFS					6
#define BIT_LEN_MASK				0x00001fc0
#define EBIT_LEN_OFS				16
#define EBIT_LEN_MASK				0x7fff0000

#define WBITS_OFFSET_OFS			0
#define WBIT_OFFSET_MASK			0x0000007f
#define E_SCAN_BIT_CNT_CFG_OFS		16
#define E_SCAN_BIT_CNT_CFG_MASK		0x003f0000

//#define SET_REG(A,B) local_write((A),(B))

//static void local_write(unsigned long  reg,unsigned long val)
//{
//    printf("addr %x, val %x\n",reg,val);
//    *(volatile unsigned long *)reg = val;
//}


static inline void write_rsa_param(unsigned long addr,unsigned long long val)
{
    SET_REG(addr,(unsigned long)((val)&0x00000000ffffffffULL));
    SET_REG(addr+4,(unsigned long)((val>>32)&0x00000000ffffffffULL));
}



static void rsa_cfg_d(unsigned long long *d,unsigned long wlen)
{
    unsigned int i;
	for (i = 0; i < wlen; i++)
	{
        write_rsa_param(RSA_REG_BASE+RSA_D_X(i), *d);
        d++;
	}
}



static void rsa_cfg_m(unsigned long long *m,unsigned long wlen)
{

    unsigned int i;
	for (i = 0; i < wlen; i++)
	{
        write_rsa_param(RSA_REG_BASE+RSA_M_X(i), *m);
        m++;
	}



}

static void rsa_cfg_n(unsigned long long *n,unsigned long wlen)
{
    
    unsigned int i;
	for (i = 0; i < wlen; i++)
	{
        write_rsa_param(RSA_REG_BASE+RSA_N_X(i), *n);
        n++;
	}


}



static void parsersadata(unsigned long long *out, void* in,unsigned long insize) 
{
    memset(out , 0, 512);
    if (insize <64) {
        /* E */
        out[0] = (unsigned long long)(*(unsigned long*)in);
    } else {
        if (insize % 64 != 0) insize = (insize/64 +1)*64;

        int i  = 0;
        char* s = in+insize/8 -1;
        char* d = out;
        for (i = 0; i<insize/8; i++ ) {
            *d = *s;
            s--;
            d++;
        }
    }
}

static void writersadata(unsigned long long *out, void* in,unsigned long insize) 
{
    memset(out , 0, insize);
    int i  = 0;
    char* s = in+insize/8 -1;
    char* d = out;\
    //printf("d %x\n",d);
    //printf("s %x\n",s);
    for (i = 0; i<insize/8; i++ ) {
        *d = *s;
        s--;
        d++;
    }
}


static unsigned long rsa_clz( unsigned long long  x )
{
    unsigned long j;
    unsigned long long mask = (unsigned long long) 1 << (0x40 - 1);

    for( j = 0; j < 0x40; j++ )
    {
        if( x & mask ) break;

        mask >>= 1;
    }

    return j;
}


static unsigned long rsa_cal_bitlen(unsigned long long * buff, unsigned long datanum) 
{
    unsigned long i, j;

    if( datanum == 0 )
        datanum = 1;

    for( i = datanum - 1; i > 0; i-- )
        if( buff[i] != 0 )
            break;
    
    j = 0x40 - rsa_clz( buff[i] );
    //printf("biL %x\n",biL);
    //printf("mbedtls_clz( X->p[i] ) %x\n",mbedtls_clz( X->p[i] ));
    //printf("i %x\n",i);
    //printf("j %x\n",j);
    
    printf("rsa_cal_bitlen %x\n", ( i * 0x40 ) + j );
    
    return( ( i * 0x40 ) + j );}

int hw_rsa_modexp(void* E,unsigned int Elen,void* N, unsigned int Nlen,void* input,void* output)
{
    unsigned int val;
    unsigned int ElenNew;
    int i = 0;
    unsigned long long writebuf[64];
    
#if 1
    printf("hw_rsa_public \n");
    printf("E : %x\n",E);
    printf("Elen : %x\n",Elen);
    printf("Nlen : %x\n",Nlen);
    printf("N: %x\n",N);
    printf("input : %x\n",input);
    printf("output : %x\n",output);
#endif



    if (Nlen != 4096 && Nlen != 2048) {
        return -1;
    }
    SET_REG_M(RSA_REG_BASE+RSA_CFG,0x0,0x8);
    SET_REG(RSA_INT+RSA_REG_BASE,0x5);
    SET_REG(RSA_INT+RSA_REG_BASE,0x1);



    if (Nlen == 4096) {
        SET_REG_M(RSA_REG_BASE+RSA_CFG,0x1,0x1);
    	SET_REG(RSA_REG_BASE+RSA_ESCAN_CFG0, 0x11303F);

    } else {
        SET_REG_M(RSA_REG_BASE+RSA_CFG,0x0,0x1);
    	SET_REG(RSA_REG_BASE+RSA_ESCAN_CFG0, 0x11301F);
    }


	val = GET_REG(RSA_REG_BASE+RSA_ESCAN_CFG1);
	val &= ~(E_SCAN_BIT_CNT_CFG_MASK | WBIT_OFFSET_MASK);	
	val |= ((63 << E_SCAN_BIT_CNT_CFG_OFS) & E_SCAN_BIT_CNT_CFG_MASK);	
	val |= ((1 << (6 - 1)) & WBIT_OFFSET_MASK);
	SET_REG(RSA_REG_BASE+RSA_ESCAN_CFG1, val);


    parsersadata(writebuf,N,Nlen);
    rsa_cfg_n(writebuf,Nlen/64);

    parsersadata(writebuf,E,Elen);
    rsa_cfg_d(writebuf,Nlen/64);

	SET_REG_M(RSA_REG_BASE+RSA_ESCAN_CFG0, rsa_cal_bitlen(writebuf,Elen/64)<<16,0xffff0000);

    parsersadata(writebuf,input,Nlen);
    rsa_cfg_m(writebuf,Nlen/64);
    
    SET_REG_M(RSA_REG_BASE+RSA_CFG,0x8,0x8);
	unsigned long long t1,t2,t3;
	
    
    while((GET_REG(RSA_INT+RSA_REG_BASE) & 0x10) != 0x10);



    writersadata(output,(void*)(RSA_REG_BASE+RSA_RESULT0),Nlen);

    SET_REG(RSA_INT+RSA_REG_BASE,0x5);
    SET_REG(RSA_INT+RSA_REG_BASE,0x1);

    printf("mol cal fin\n");
}



